#ifndef EXTRACT_LANE_HPP
#define EXTRACT_LANE_HPP

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <tf/transform_listener.h>


typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> CloudT;
typedef pcl::PointCloud<PointT>::Ptr CloudPtr;

void on_mouse(int event,int x,int y,int flsgs,void* ustc); 

class ExtractLane{
public:
    ExtractLane(ros::NodeHandle& n)
    {
        std::vector<int> low_thres,high_thres;
        n.getParam("search_area",search_area_);
        n.getParam("low_thres",low_thres);
        n.getParam("high_thres",high_thres);
        n.getParam("downsample_rate",downsample_rate_);
        n.getParam("show_image",show_image_);
        ROS_ASSERT(low_thres.size() == 3);
        ROS_ASSERT(high_thres.size() == 3);
        ROS_ASSERT(search_area_.size() == 2);
        ROS_ASSERT(downsample_rate_.size() == 2);
        low_thres_ = cv::Scalar(low_thres[0],low_thres[1],low_thres[2]);
        high_thres_ = cv::Scalar(high_thres[0],high_thres[1],high_thres[2]);
        camera_height_ = 0.2;
        set_camera_matrix_ = false;
        set_transform_matrix_ = false;
    }

    void set_camera_matrix(Eigen::Matrix3d& camera_mat)
    {
        camera_matrix_ = camera_mat;
        set_camera_matrix_ = true;
    }

    void set_transform(Eigen::Matrix4d& transform_matrix)
    {
        transform_matrix_ = transform_matrix;
        set_transform_matrix_ = true;
    }

    void process(cv::Mat& src)
    {
        if(!set_camera_matrix_ || !set_transform_matrix_)
            return ;
        lane_cloud_ptr_.reset(new CloudT);
        int width = src.cols;
        int height = src.rows;
        int top = std::min(height,search_area_[1]);
        int bottom = std::max(0,search_area_[0]);
        cv::Mat binary = cv::Mat::zeros(height,width,CV_8UC1);
        cv::Mat hsv_image;
        cv::cvtColor(src,hsv_image,cv::COLOR_BGR2HSV);
        for(int i=bottom;i<top;i++)
        {
            for(int j=0;j<width;j++)
            {
                if(satisfy_color(hsv_image.at<cv::Vec3b>(i,j)))
                {
                    binary.at<uchar>(i,j) = 255;
                }
            }
        }
        auto kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3),cv::Point(-1,-1));
        cv::dilate(binary,binary,kernel,cv::Point(-1,-1),1);    //膨胀  
        cv::erode(binary,binary,kernel);        //腐蚀
        fitLane(binary);
        // downsample(binary,src);
        // CloudPtr pt_base(new CloudT());
        // lane_cloud_ptr_->width = lane_cloud_ptr_->points.size();   //必须赋值
        // lane_cloud_ptr_->height = 1;
        // pcl::transformPointCloud(*lane_cloud_ptr_,*pt_base,transform_matrix_);
        // pt_base->width = pt_base->points.size();
        // pt_base->height = 1;
        // lane_cloud_ptr_ = pt_base;
        // if(show_image_)
        // {
        //     cv::namedWindow("origin");
        //     cv::setMouseCallback("origin",on_mouse,this);
        //     cv::imshow("origin",src);
        //     cv::waitKey(1);
        // }
    }

    CloudPtr get_lane_cloud()
    {
        return lane_cloud_ptr_;
    }
private:

    friend void on_mouse(int event,int x,int y,int flsgs,void* ustc); 

    bool satisfy_color(cv::Vec3b& pixel)
    {
        return ((pixel[0]>=low_thres_[0] && pixel[0]<=high_thres_[0]) && (pixel[1]>=low_thres_[1] && pixel[1]<=high_thres_[1]) && (pixel[2]>=low_thres_[2] && pixel[2]<=high_thres_[2]));
    }

    int calculate_theory_minium_pixels(int i)    //利用车道线宽度为10cm的假设和相机内参计算应该当前行的车道线最少应该连续多少个点
    {
        int y_y0 = abs(i-camera_matrix_(1,2));
        int delta_x = 0.05/camera_height_*y_y0;
        return delta_x;
    }

    void downsample(cv::Mat& binary,cv::Mat& origin)     //去除错误识别
    {
        int x_downsample = downsample_rate_[0];   //每隔三个像素点作一次采样
        int y_downsample = downsample_rate_[1];   
        int width = origin.cols;
        int height = origin.rows;
        int top = std::min(height,search_area_[1]);
        int bottom = std::max(0,search_area_[0]);
        for(int i=bottom;i<top;i=i+x_downsample)
        {
            int min_x = calculate_theory_minium_pixels(i);
            min_x*=0.9;
            min_x = min_x/y_downsample-1;     //配合降采样     
            std::vector<int> pt;
            for(int j=0;j<width;j=j+y_downsample)
            {
                if(binary.at<uchar>(i,j) == 255)
                    pt.emplace_back(j);
                if(binary.at<uchar>(i,j) == 0 && pt.size() != 0)
                {
                    if(pt.size()>=min_x)
                    {
                        cv::circle(origin,cv::Point(pt[pt.size()/2],i),2,cv::Scalar(255,0,0),2);
                        for(int k=0;k<pt.size();k++)
                            save_point(pt[k],i);
                        pt.clear();
                    }
                    else
                    {
                        for(int k=0;k<pt.size();k++)
                        {
                            cv::circle(origin,cv::Point(pt[k],i),1,cv::Scalar(0,255,0));
                        }
                        pt.clear();
                    }
                }
            }
            if(pt.size()>min_x)
            {
                cv::circle(origin,cv::Point(pt[pt.size()/2],i),2,cv::Scalar(255,0,0),2);
                for(int k=0;k<pt.size();k++)
                    save_point(pt[k],i);                
                pt.clear();
            }
            else
                pt.clear();
        }
    }

    void save_point(int u,int v)      //车道线点变换到3d中存入点云
    {
        lane_cloud_ptr_->points.emplace_back(calculate_depth(u,v));
    }

    PointT calculate_depth(int u,int v)
    {
        PointT p;
        double z = -camera_height_;
        double x = (u-camera_matrix_(0,2))/(v-camera_matrix_(1,2))*camera_height_;
        double y = camera_matrix_(1,1)*camera_height_/(v-camera_matrix_(1,2));
        p.x = x;
        p.y = y;
        p.z = z;
        return p;
    }

    void fitLane(cv::Mat& src_image)
    {
        std::vector<std::vector<cv::Point>> contours;
    
    
        cv::Mat dstImg(src_image.size(), CV_8UC3, cv::Scalar::all(0));//纯黑图像  

        //使用canny检测出边缘
    
        cv::Mat edge_image;
    
        cv::Canny(src_image,edge_image,30,70);
    
        cv::imshow("canny边缘",edge_image);

        cv::waitKey(1);
    
    
    
        //边缘追踪，没有存储边缘的组织结构
    
        findContours(edge_image, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
    
        cv::Mat cimage = cv::Mat::zeros(edge_image.size(), CV_8UC3);
    
    
        std::vector<std::vector<cv::Point>> contours_poly(contours.size());//用于存放折线点集
    
        for (int i = 0; i<contours.size(); i++)
        {
            cv::approxPolyDP(cv::Mat(contours[i]), contours_poly[i], 5, true);
            drawContours(dstImg, contours_poly, i, cv::Scalar(0, 255, 255), 2, 8);  //绘制
        }
        imshow("approx", dstImg);

    }





private:
    std::vector<int> search_area_;
    std::vector<int> downsample_rate_;
    cv::Scalar low_thres_;
    cv::Scalar high_thres_;
    Eigen::Matrix3d camera_matrix_;
    Eigen::Matrix4d transform_matrix_;
    bool set_camera_matrix_;
    bool set_transform_matrix_;
    bool show_image_;
    double camera_height_;
    CloudPtr lane_cloud_ptr_;

};


inline void on_mouse(int event,int x,int y,int flsgs,void* ustc)
{
    if(event == CV_EVENT_FLAG_LBUTTON)
    {    
        ExtractLane* extract_lane = static_cast<ExtractLane*>(ustc);
        extract_lane->calculate_depth(x,y);
    }
}







#endif